#include <Bounce.h>

const int sensRR = A2;
const int sensMR = A3;
const int sensML = A4;
const int sensLL = 6;
const int encR = 3; const int encRLED = 11;
const int encL = 2; const int encLLED = 12;
const int sensBR = 12;
const int sensBL = 11;

const int ddelay = 50;

Bounce RR = Bounce(sensRR, ddelay); 
Bounce MR = Bounce(sensMR, ddelay); 
Bounce ML = Bounce(sensML, ddelay); 
Bounce LL = Bounce(sensLL, ddelay); 
Bounce BL = Bounce(sensBL, ddelay); 
Bounce BR = Bounce(sensBR, ddelay); 

int count = 0;
int lastSens;

void sensSetup(){
  pinMode(sensLL, INPUT);
  pinMode(encR, INPUT);
  pinMode(encL, INPUT);
  pinMode(encRLED, OUTPUT);
  pinMode(encLLED, OUTPUT);
  pinMode(sensBL, INPUT);
  pinMode(sensBR, INPUT);
  attachInterrupt(1, encoderR, CHANGE);
  attachInterrupt(0, encoderL, CHANGE);
}

int sens(){  //1=right; 2=MR; 3=ML; 4=Left; 5=BL; 6=BR
  int val = 0;
  int res = 0;
  
  //Taster ohne PullUp Widerstand
  pinMode(sensBL, OUTPUT);
  pinMode(sensBR, OUTPUT);
  digitalWrite(sensBL, HIGH);
  digitalWrite(sensBR, HIGH);
  pinMode(sensBL, INPUT);
  pinMode(sensBR, INPUT); 
  
  RR.update();
  MR.update();
  ML.update();
  LL.update();
  BR.update();
  BL.update();
  
  val = RR.read();
  if(val == LOW) {res = 1;}
  else{
    val = MR.read();
    if(val == LOW) {res = 2;}
    else{
      val = ML.read();
      if(val == LOW) {res = 3;}
      else{
        val = LL.read();
        if(val == LOW) {res = 4;} 
      }
    }
  }
  
  //Back sensoren setzten sich durch
  val = BL.read();
  if(val == LOW) {res = 5;}  
  else{
    val = BR.read();
    if(val == LOW) {res = 6;} 
  } 

  /*
  count++;
  if((millis()-startTime)>1000){
    Serial.print("Sens: ");
    Serial.print(count);
    Serial.println("mal pro Sek");
    delay(2);
    count=0;
    startTime=millis(); 
  } */
  
  if(lastSens != res){
    Serial.print("Sens: ");Serial.println(res);   
    lastSens = res;
    delay(1);
  }
  return res;
}

void encoderL(){
  iencL++;
//  Serial.print("encL: ");  
//  Serial.println(iencL);
}

void encoderR(){
  iencR++;
//  Serial.print("encR: ");  
//  Serial.println(iencR);
} 

